﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_Objects2Real.cs
 * \brief	This file contains the Objects2Real function that 
 *  
 * This file contains the Objects2Real function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodríguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.04
 * \date      2010-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-11-20 \n
 *      First implementation of the M44_Objects2Real class.
 *      
 * - Version 0.02:   Gonzalo RODRIGUEZ                      2010-12-15 \n
 *      Update and code errors. 
 *      
 * - Version 0.03: GOnzalo RODRIGUEZ                        2010-02-17
 *      Remove Additional Info structure, no longer needed
 *      
 *  -Version 0.04: Gonzalo RODRIGUEZ                          2010-06-23 \n
 *      Add the constants that control the whole dynamic object detection algorithm
 *      
/* ######################################################################### */
using System;
using System.Collections.Generic;
using System.Text;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/*This is for using lists*/
using System.Collections;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/* M44_SharedClasses */
using NMRS.M44_SharedClasses;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_Objects2Real
     * 
     *  \brief  This is the class of the M44_Objects2Real function.
     *
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_Objects2Real
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static DetectedObjects ObjectFinder( float[,] DistanceDiference )
         * 
         *  \brief 
         *  
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] int num                              Number of objects
         *  \param[in] in[,] Objects                        Matrix containing object information
         *  \param[in] float[,] tf_Dref                     Reference matrix
         *  \param[in] cl_RobotEstimate RobotEstimate       Robot Position Estimate
         *  \param[in] cl_Pos3D[,] CollisionMatrix
         *  
         *  \param[out] RobotObjectList str_RobotObjectList structure containing cl_DynamicObjects class and the additional info
         */
        /* ------------------------------------------------------------------------- */
        public static RobotObjectList Objects2Real(cl_3DLrfMeasure Lrf3DMeasure,
                                                        DetectedObjects str_DetectedObjects,
                                                        float[,] tf_Dref,
                                                        cl_RobotEstimate RobotEstimate,
                                                        cl_Pos3D[,] CollisionMatrix,
                                                        float ActTime, char[] RobotID
                                                      )
        {

            /*Variables*/
            RobotObjectList str_RobotObjectList = new RobotObjectList(30);
            int[,] index = new int[1000, 2];
            int n_px = 0;
            int pos_i;
            int pos_j;
            int i_aux = 0;
            float mean_i = 0;
            float mean_j = 0;
            float difference_tol = DynObj_Const.f_laser_reference_dif_tol;
            float xmin = 100000;
            float xmax = 0;
            float ymin = 100000;
            float ymax = 0;
            float zmax = 0;
            float zmin = 100000;
            float XCollision, YCollision, ZCollision;
            float x_tol = DynObj_Const.f_x_size_tol;
            float y_tol = DynObj_Const.f_y_size_tol;
            float z_tol = DynObj_Const.f_z_size_tol;
            float f_size;
            int i_obj_added = 0;
            int[,] Objects = str_DetectedObjects.ti_Objects;
            /*Structure to store data*/
            str_RobotObjectList.DynamicObjects = new cl_DynamicObjects((ushort)str_DetectedObjects.i_NumberOfObjects);
            cl_DynamicObjects DynObjaux; 
            for (int i_obj = 0; i_obj < str_DetectedObjects.i_NumberOfObjects; i_obj++)
            {
                index = new int[100000, 2];
                n_px = 0;
                i_aux = 0;
                for (int i = 0; i < Lrf3DMeasure.s_i; i++)
                {
                    for (int j = 0; j < Lrf3DMeasure.s_n; j++)
                    {
                        if (Objects[i, j] == (int)str_DetectedObjects.s_labels[i_obj])
                        {
                            index[i_aux, 0] = i;
                            index[i_aux, 1] = j;
                            n_px++;
                            i_aux++;
                        }
                    }
                }
                        /* Check all the points in the object to see if 
                         * they are part of the object itself or the 
                         * background.
                         */
                mean_i = 0;
                mean_j = 0;
                xmin = 100000;
                xmax = -100000;
                ymin = 100000;
                ymax = -100000;
                zmin = 100000;
                zmax = -100000;
                for (int i = 0; i < n_px; i++)
                {
                    pos_i = index[i, 0];
                    pos_j = index[i, 1];
                    

                    if (Math.Abs(Lrf3DMeasure.tf_3DLrfDistance[pos_i, pos_j] - tf_Dref[pos_i, pos_j]) > difference_tol)
                    {
                        mean_i = mean_i + pos_i;
                        mean_j = mean_j + pos_j;
                        /*The point belongs to the object*/
                        if (CollisionMatrix[pos_i, pos_j] != null)
                        {
                            XCollision = CollisionMatrix[pos_i, pos_j].f_x;
                            YCollision = CollisionMatrix[pos_i, pos_j].f_y;
                            ZCollision = CollisionMatrix[pos_i, pos_j].f_z;

                            /*They are stored in*/
                            xmin = Math.Min(XCollision, xmin);
                            xmax = Math.Max(XCollision, xmax);
                            ymin = Math.Min(YCollision, ymin);
                            ymax = Math.Max(YCollision, ymax);
                            zmin = Math.Min(ZCollision, zmin);
                            zmax = Math.Max(ZCollision, zmax);
                        }
                    }
                }

                if ((xmax - xmin) > x_tol || (ymax - ymin) > y_tol || (zmax - zmin) > z_tol)
                {

                    f_size = Math.Max(Math.Max(Math.Abs(xmax - xmin), Math.Abs(ymax - ymin)), Math.Abs(zmax - zmin));
                    str_RobotObjectList.DynamicObjects.List[i_obj_added] = new Interfaces.Complex.cl_DynamicObjectList();

                    str_RobotObjectList.DynamicObjects.List[i_obj_added].PhysCharac.f_PhysSize = f_size;
                    //Calculate the position of the center of the robot
                    //Position
                    pos_i = (int)Math.Round((double)mean_i / n_px);
                    pos_j = (int)Math.Round((double)mean_j / n_px);
                    if (CollisionMatrix[pos_i, pos_j] != null)
                    {
                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_x = CollisionMatrix[pos_i, pos_j].f_x;
                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_y = CollisionMatrix[pos_i, pos_j].f_z;
                    }
                    else
                    {
                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_x = 0;
                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_y = 0;
                    }
                    str_RobotObjectList.DynamicObjects.List[i_obj_added].f_DetectionTime = ActTime;
                    i_obj_added++;
                }
            }
            DynObjaux = new cl_DynamicObjects((ushort)i_obj_added);
            i_aux = 0;
            int n1 = Convert.ToInt32(Convert.ToString(RobotID[7]));
            int n2 = Convert.ToInt32(Convert.ToString(RobotID[8]));
            ushort Obj_ID = (ushort)(n1 * 10000 + n2 * 1000);
            for (int i = 0; i < str_DetectedObjects.i_NumberOfObjects; i++)
            {
                if (str_RobotObjectList.DynamicObjects.List[i] != null)
                {
                    DynObjaux.List[i_aux] = str_RobotObjectList.DynamicObjects.List[i];
                    DynObjaux.List[i_aux].us_Identification = (ushort)(Obj_ID+1);
                    Obj_ID++;
                    i_aux++;

                }
            }
            str_RobotObjectList.DynamicObjects = DynObjaux;

            return str_RobotObjectList;
        }
    }
}
